#bgr8转jpeg格式
import enum
import cv2

def bgr8_to_jpeg(value, quality=75):
    return bytes(cv2.imencode('.jpg', value)[1])

#摄像头组件显示
import traitlets
import ipywidgets.widgets as widgets
import time
#线程功能操作库
import threading
import inspect
import ctypes

origin_widget = widgets.Image(format='jpeg', width=320, height=240)
mask_widget = widgets.Image(format='jpeg',width=320, height=240)
result_widget = widgets.Image(format='jpeg',width=320, height=240)

# create a horizontal box container to place the image widget next to eachother
image_container = widgets.HBox([origin_widget, mask_widget, result_widget])

display(image_container)

#线程相关函数
def _async_raise(tid, exctype):
    """raises the exception, performs cleanup if needed"""
    tid = ctypes.c_long(tid)
    if not inspect.isclass(exctype):
        exctype = type(exctype)
    res = ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, ctypes.py_object(exctype))
    if res == 0:
        raise ValueError("invalid thread id")
    elif res != 1:
        # """if it returns a number greater than one, you're in trouble,
        # and you should call it again with exc=NULL to revert the effect"""
        ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, None)
        
def stop_thread(thread):
    _async_raise(thread.ident, SystemExit)

    import cv2
import numpy as np
import ipywidgets.widgets as widgets
import RPi.GPIO as GPIO
import time

cap = cv2.VideoCapture(0)
cap.set(3, 640)
cap.set(4, 480)
cap.set(5, 120)  #设置帧率
cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter.fourcc('M', 'J', 'P', 'G'))
cap.set(cv2.CAP_PROP_BRIGHTNESS, 20) #设置亮度 -64 - 64  0.0
cap.set(cv2.CAP_PROP_CONTRAST, 20)   #设置对比度 -64 - 64  2.0
# image.set(cv2.CAP_PROP_EXPOSURE, 156)  #设置曝光值 1.0 - 5000  156.0

#小车电机引脚定义，复制到这里
IN1 = 20
IN2 = 21
IN3 = 19
IN4 = 26
ENA = 16
ENB = 13

#小车按键定义
key = 8

#循迹红外引脚定义
#TrackSensorLeftPin1 TrackSensorLeftPin2 TrackSensorRightPin1 TrackSensorRightPin2
#      3                 5                  4                   18
TrackSensorLeftPin1  =  3   #定义左边第一个循迹红外传感器引脚为3口
TrackSensorLeftPin2  =  5   #定义左边第二个循迹红外传感器引脚为5口
TrackSensorRightPin1 =  4   #定义右边第一个循迹红外传感器引脚为4口
TrackSensorRightPin2 =  18  #定义右边第二个循迹红外传感器引脚为18口

#设置GPIO口为BCM编码方式
GPIO.setmode(GPIO.BCM)

#忽略警告信息
GPIO.setwarnings(False)#复制结束点

# 默认选择红色的，想识别其他请注释下面红色区间代码，放开后面其他区间代码段
# 红色区域
#color_lower = np.array([0, 43, 46])
#color_upper = np.array([10, 255, 255])

# #绿色区间
# color_lower = np.array([35, 43, 46])
# color_upper = np.array([77, 255, 255])

# #蓝色区间
color_lower=np.array([100, 43, 46])
color_upper = np.array([124, 255, 255])

# #黄色区间
# color_lower = np.array([26, 43, 46])
# color_upper = np.array([34, 255, 255])

# #橙色区间
# color_lower = np.array([11, 43, 46])
# color_upper = np.array([25, 255, 255])

is_detected = False 

#定义复制起始
def init():
    global pwm_ENA
    global pwm_ENB
    GPIO.setup(ENA,GPIO.OUT,initial=GPIO.HIGH)
    GPIO.setup(IN1,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(IN2,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(ENB,GPIO.OUT,initial=GPIO.HIGH)
    GPIO.setup(IN3,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(IN4,GPIO.OUT,initial=GPIO.LOW)
    GPIO.setup(key,GPIO.IN)
    GPIO.setup(TrackSensorLeftPin1,GPIO.IN)
    GPIO.setup(TrackSensorLeftPin2,GPIO.IN)
    GPIO.setup(TrackSensorRightPin1,GPIO.IN)
    GPIO.setup(TrackSensorRightPin2,GPIO.IN)
    #设置pwm引脚和频率为2000hz
    pwm_ENA = GPIO.PWM(ENA, 2000)
    pwm_ENB = GPIO.PWM(ENB, 2000)
    pwm_ENA.start(0)
    pwm_ENB.start(0)
        
#小车前进	
def run(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车后退
def back(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

def runone(delaytime):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)

#小车后退
def backone(delaytime):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(35)
    pwm_ENB.ChangeDutyCycle(35)
    time.sleep(delaytime)
    
#小车左转	
def left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车右转
def right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)
        
#小车原地左转
def spin_left(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.HIGH)
    GPIO.output(IN3, GPIO.HIGH)
    GPIO.output(IN4, GPIO.LOW)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车原地右转
def spin_right(leftspeed, rightspeed):
    GPIO.output(IN1, GPIO.HIGH)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.HIGH)
    pwm_ENA.ChangeDutyCycle(leftspeed)
    pwm_ENB.ChangeDutyCycle(rightspeed)

#小车停止	
def brake():
    GPIO.output(IN1, GPIO.LOW)
    GPIO.output(IN2, GPIO.LOW)
    GPIO.output(IN3, GPIO.LOW)
    GPIO.output(IN4, GPIO.LOW)

#按键检测
def key_scan():
    while GPIO.input(key):
        pass
    while not GPIO.input(key):
        time.sleep(0.01)
        if not GPIO.input(key):
            time.sleep(0.01)
            while not GPIO.input(key):
                pass

#延时2s	
time.sleep(2)

    #定义复制结束

def Color_Recongnize():
    
    global is_detected
    
    while(1):
        # get a frame and show 获取视频帧并转成HSV格式, 利用cvtColor()将BGR格式转成HSV格式，参数为cv2.COLOR_BGR2HSV。
        ret, frame = cap.read()
        #cv2.imshow('Capture', frame)
        origin_widget.value = bgr8_to_jpeg(frame)

        # change to hsv model
        hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)

        # get mask 利用inRange()函数和HSV模型中蓝色范围的上下界获取mask，mask中原视频中的蓝色部分会被弄成白色，其他部分黑色。
        mask = cv2.inRange(hsv, color_lower, color_upper)
        #cv2.imshow('Mask', mask)
        mask_widget.value = bgr8_to_jpeg(mask)

        # detect blue 将mask于原视频帧进行按位与操作，则会把mask中的白色用真实的图像替换：
        res = cv2.bitwise_and(frame, frame, mask=mask)
        #cv2.imshow('Result', res)
        result_widget.value = bgr8_to_jpeg(res)
        
        if cv2.countNonZero (mask) > 10:
            is_detected = True
        else:
            is_detected = False
        

        #     if cv2.waitKey(1) & 0xFF == ord('q'):
        #         break
        time.sleep(0.01)


    cap.release()
    #cv2.destroyAllWindows()

    #启动进程
thread1 = threading.Thread(target=Color_Recongnize)
thread1.setDaemon(True)
thread1.start()

#try/except语句用来检测try语句块中的错误，
#从而让except语句捕获异常信息并处理。
try:
    init()
    key_scan()
    #寻找到宝藏1
    start_time1=time.time()
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)    
        if TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(19, 19)     
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0)
        current_time1 = time.time()
        if current_time1 - start_time1 >=14.7:
            break
    if is_detected == True:
        while True:
            runone(0.2)
            backone(0.4)
            break
        start_time1_1 = time.time()
        while True:
            TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
            TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
            TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
            TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
            spin_left(40,40)
            current_time1_1 = time.time()
            if current_time1_1 - start_time1_1 >= 0.85:
                break
   #寻找到宝藏2
    start_time2=time.time()
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)    
        if TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(13, 13)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0) 
        current_time2 = time.time()
        if current_time2 - start_time2 >=13:
            break
    while True:
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
        if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(15, 15)  
        elif TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0)    
        current_time2 = time.time()
        if current_time2 - start_time2 >=21.5:
            break
        
    if is_detected == True:
        while True:
            runone(0.2)
            backone(0.4)
            break  
        start_time2_1 = time.time()
        while True:
            TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
            TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
            TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
            TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
            spin_left(40,40)
            current_time2_1 = time.time()
            if current_time2_1 - start_time2_1 >= 0.88:
                break
    #寻找到宝藏3
    '''start_time3=time.time()
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)    
        if TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(15, 15)   
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        current_time3 = time.time()
        if current_time3 - start_time3 >=2:
            break
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
        if TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(15, 15)   
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0) 
        current_time3_1 = time.time()
        if current_time3_1 - start_time3 >=8:
            break       
    time.sleep(1.5)
    start_time4=time.time()    
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)    
        if TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(13, 13)   
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0)
        current_time4 = time.time()
        if current_time4 - start_time4 >=9:
            break
    if is_detected == True:
        while True:
            runone(0.5)
            backone(0.5)
            break
        start_time4_1 = time.time()
        while True:
            TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
            TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
            TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
            TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)
            spin_left(40,40)
            current_time4_1 = time.time()
            if current_time4_1 - start_time4_1 >= 0.85:
                break
    #chumigong
    start_time3=time.time()
    while True:
        #检测到黑线时循迹模块相应的指示灯亮，端口电平为LOW
        #未检测到黑线时循迹模块相应的指示灯灭，端口电平为HIGH
        TrackSensorLeftValue1  = GPIO.input(TrackSensorLeftPin1)
        TrackSensorLeftValue2  = GPIO.input(TrackSensorLeftPin2)
        TrackSensorRightValue1 = GPIO.input(TrackSensorRightPin1)
        TrackSensorRightValue2 = GPIO.input(TrackSensorRightPin2)    
        if TrackSensorLeftValue1 == False:
            spin_left(40, 40)
        elif TrackSensorRightValue2 == False:
            spin_right(40, 40)
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == False:
            run(15, 15)     
        elif TrackSensorLeftValue2 == False and TrackSensorRightValue1 == True:
            left(0,35)
        elif TrackSensorLeftValue2 == True and TrackSensorRightValue1 == False:
            right(35, 0)
        current_time3 = time.time()
        if current_time3 - start_time3 >=15:
            break'''
except KeyboardInterrupt:
    pass
pwm_ENA.stop()
pwm_ENB.stop()
GPIO.cleanup